#include "chassis_power_control.h"
#include "app_preference.h"
#include "tasks.h"
#include "LQR.h"
#include "Leg.h"
#include "Mpc.h"

PowerClass::PowerClass()
{
	
	Power_K.K1=POWER_K1;
	Power_K.K2=POWER_K2;
	Power_K.constant=POWER_CONSTANT;
	cap_state=0;
  power_buffer_set=30;
  Power_K.Cap_power_open=CAP_POWER_OPEN;
  Power_K.Cap_power_close=CAP_POWER_CLOSE;
	Power_calc.chassis_give_current[0]=Power_calc.chassis_give_current[1]=0;
	Power_limit.RPM_set_limit[0]=Power_limit.RPM_set_limit[1]=0;

	Power_calc.initial_send_power[0]=Power_calc.initial_send_power[1]=0;
	Power_calc.scaled_give_power[0]=Power_calc.scaled_give_power[1]=0;
	
	Power_calc.initial_send_total_power=Power_calc.initial_send_total_power=0;
	Power_calc.scaled_total_power=Power_calc.scaled_total_power=0;
	Power_limit.RPM_set_total_limit_Max=Power_limit.RPM_set_total_limit_Max=0;
  power_scale=0;
	

}

void PowerClass::Power_Feedback_Update()
{
	float a,b,c,temp;
	
	#ifdef CPXT
		Power_limit.grade_power_limit=Message.robo->game_robot_state.chassis_power_limit;
		Power_limit.Max_input_power =(Power_limit.grade_power_limit);//+Power_limit.power_buffer_out;//,最大功率更新，缓冲能量环输出+该等级功率上限
	#endif
	#ifdef debug
		Power_limit.grade_power_limit=90;
		Power_limit.Max_input_power=60; //120最大功率更新，缓冲能量环输出+该等级功率上限
	#endif

  //转速反馈
  Power_calc.chassis_speed_rpm[0]= Chassis.Wheel_Motor[0].chassis_motor_measure->speed_rpm;
	Power_calc.chassis_speed_rpm[1]=-Chassis.Wheel_Motor[1].chassis_motor_measure->speed_rpm;
	
	Power_calc.chassis_give_current[0]= Chassis.Wheel_Motor[0].chassis_motor_measure->given_current;
	Power_calc.chassis_give_current[1]=-Chassis.Wheel_Motor[1].chassis_motor_measure->given_current;

}
void PowerClass::Power_Calc()
{
	uint8_t i;
  float a_I,b_I,c_I,temp_I;
  float a_n,b_n,c_n,temp_n;

	//,最大功率计算
	if(Message.SuperCapR.energy>20)//,百分比大于20%时超部分功率
	{
		
	 if(cap_state==1)
	 {
		 Power_limit.Chassis_Max_power=Power_limit.Max_input_power+Power_K.Cap_power_open;//，开时大超
	 }
	 else
	 {
     Power_limit.Chassis_Max_power=Power_limit.Max_input_power+Power_K.Cap_power_close;//,不开时小超
	 }
	}
	else
	{
		Power_limit.Chassis_Max_power=Power_limit.Max_input_power;//，电容没电时不超
	}
	//，计算当前功率
	for( i=0;i<2;i++)
	{
		Power_calc.initial_send_power[i]//,当前电机功率模型计算
		=(TOQUE_COEFFICIENT*fabs(Power_calc.send_current_value[i])*fabs(Power_calc.chassis_speed_rpm[i]))//,力矩和转速（力矩使用电流发送值代替，因为其是线性关系）
		+Power_K.K1*Power_calc.send_current_value[i]*Power_calc.send_current_value[i]//,力矩平方
		+Power_K.K2*Power_calc.chassis_speed_rpm[i]*Power_calc.chassis_speed_rpm[i];//,转速平方
	}
  Power_calc.initial_send_total_power=Power_calc.initial_send_power[0]+Power_calc.initial_send_power[1];//,当前总功率（两轮加起来）
	
	LQR_Power_calc.K_s=-0.5*(LQR_Ctrl.Wheel_Kgain1[0][1]+LQR_Ctrl.Wheel_Kgain1[1][1]);
	LQR_Power_calc.K_q= 0.75*(LQR_Ctrl.Wheel_Kgain1[1][2]-LQR_Ctrl.Wheel_Kgain1[0][2]);
	
	LQR_Power_calc.LQR_total_Power=2*Power_K.K1*CHASSIS_MOTOR_TORQUE_TO_CURRENT_SEN*CHASSIS_MOTOR_TORQUE_TO_CURRENT_SEN*(LQR_Power_calc.K_s*(Leg_Ctrl.Leg_set.Wheel_Speed_set-LQR_Ctrl.Leg_state.Distance_dot)*(Leg_Ctrl.Leg_set.Wheel_Speed_set-LQR_Ctrl.Leg_state.Distance_dot)+ 0.75 *(LQR_Ctrl.Leg_state.phi)* 0.75 *(LQR_Ctrl.Leg_state.phi))
																 + TOQUE_COEFFICIENT*WHEEL_RADIUS*CHASSIS_MOTOR_TORQUE_TO_CURRENT_SEN*(LQR_Power_calc.K_s*(fabs(LQR_Ctrl.Leg_state.Distance_dot*2.0f/WHEEL_RADIUS*((Leg_Ctrl.Leg_set.Wheel_Speed_set-LQR_Ctrl.Leg_state.Distance_dot)))) +LQR_Power_calc.K_q*((-Leg_Ctrl.Vmc_Ctrl[0].Wheel_Speed_Estimate+Leg_Ctrl.Vmc_Ctrl[1].Wheel_Speed_Estimate)*LQR_Ctrl.Leg_state.phi ) )
																 + Power_K.K2*WHEEL_RADIUS*(fabs(LQR_Ctrl.Leg_state.Distance_dot*2.0f/WHEEL_RADIUS));
	
	//做功率限制和分配
	if((Power_calc.initial_send_total_power)>Power_limit.Chassis_Max_power)//，超出最大功率限制
	{
				float a,b,c,temp;
		float power_scale=Power_limit.Chassis_Max_power/Power_calc.initial_send_total_power;//，缩放系数计算
	  for(uint8_t i=0;i<2;i++)
		{
      Power_calc.scaled_give_power[i]=Power_calc.initial_send_power[i]*power_scale;//,缩放后的最大功率
			if(Power_calc.scaled_give_power<0)
				return ;
      Power_calc.scaled_total_power=Power_calc.scaled_give_power[0]+Power_calc.scaled_give_power[1];

			 a=Power_K.K1;//,二元一次方程的a
			 b=TOQUE_COEFFICIENT*Power_calc.chassis_speed_rpm[i];//，二元一次方程的b
			 c=Power_K.K2*Power_calc.chassis_speed_rpm[i]*Power_calc.chassis_speed_rpm[i]-Power_calc.scaled_give_power[i]+Power_K.constant;//，二元一次方程的c
		
			if(Power_calc.send_current_value[i]>0)//,向前走取正解
			{
				 temp=(-b+sqrt(b*b-4*a*c))/(2*a);//,二元一次方程的正解
				if(temp>16000)//，限制最大解
					Power_calc.send_current_value[i]=16000;
				else
					Power_calc.send_current_value[i]=temp;
			}
			else//,向后走取负解
			{
				 temp=(-b-sqrt(b*b-4*a*c))/(2*a);//,二元一次方程的负解
				if(temp<-16000)//，限制最大解
					Power_calc.send_current_value[i]=-16000;
				else
					Power_calc.send_current_value[i]=temp;		
			}
	  }
		
	}
}

